/**
 * Copyright (C) 2011  E-Robot
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>
 *
 * @brief Robot.cpp
 *
 * @date 20 oct. 2010
 * @author Elias Capito
 * @version 0.1
 */

#include "Robot.h"
#include <qdebug.h>
#include "Sonar.h"
#include <math.h>
#include <string.h>
#include <QTime>
Robot::Robot(string _modelName,string _host, int _port)
{
	m_state = AnyAlgo;

	m_client  = new PlayerClient(_host, _port);
	m_simProxy = new SimulationProxy(m_client,0);
	m_p2dProxy = new Position2dProxy(m_client,0);
	m_sonarProxy = new SonarProxy(m_client,0);


	m_modelName = new char(_modelName.length()+1);
	strcpy(m_modelName,_modelName.c_str());
	m_modelName[_modelName.length()] = '\0';
	m_map =  NULL;

	createSonar();
}

Robot::~Robot()
{

}

void Robot::useMap(Map* _map){
	m_map = _map;
}

void Robot::changeAlgo(){
	changeState((AlgoEnum)((m_state+1)%4));
}

void Robot::changeState(AlgoEnum _algo){

	m_state = _algo;
	switch(m_state){
	case Random:
		cout<< "Move randomly"<<endl;
			break;
	case FollowAtLeft:
		cout<< "Following left wall"<<endl;
		break;
	case FollowAtRight:
		cout<< "Following right wall"<<endl;
			break;
	default:
		cout<< "Any algorithm used !"<<endl;
		break;
	}
}

void Robot::updateRobotPosition(){
	m_sonarProxy->RequestGeom();
	m_simProxy->GetPose2d(m_modelName,m_x,m_y,m_angle);
}

void Robot::updateSonar(){

	m_client->Read(); //Method du client pour mettre à jour les capteurs
	m_client->Read(); //Method du client pour mettre à jour les capteurs
	m_client->Read();

	static double x = 0;
	static double y = 0;
	static double a =0;
	static double d =0;
	static QLineF line;

	updateRobotPosition();
	//Parcours chaque capteur et met à jour la distance

	unsigned int nbSonar = m_sonars.size();
	for(unsigned int i = 0; i<nbSonar;i++){

		d = m_sonarProxy->GetScan(i);
		 if( d>=0){
			 m_sonars.at(i)->setDistance(d);
		}
	}

	for(unsigned int i = 0; i<nbSonar;i++){

	//	if(i==4){
		d= m_sonars.at(i)->distance();
		x = m_x+ m_sonars.at(i)->vectorRobotCenterToCapteur().x()*cos(m_angle)- m_sonars.at(i)->vectorRobotCenterToCapteur().y()*sin(m_angle);
		y = m_y+ m_sonars.at(i)->vectorRobotCenterToCapteur().x()*sin(m_angle)+ m_sonars.at(i)->vectorRobotCenterToCapteur().y()*cos(m_angle);
		a =m_angle+ m_sonars.at(i)->angle();

		line.setLine(x,y,x+cos(a)*(d),y+sin(a)*(d));
		Q_EMIT(updateMap(line));
		//}
	}


}
void Robot::createSonar(){
	m_sonarProxy->RequestGeom();

	//Création des sonars avec leurs propres vecteurs
	//0
	Sonar* s = new Sonar(0,m_sonarProxy->GetPose(0),Right);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,20));
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0,25));

	//1
	s = new Sonar(1,m_sonarProxy->GetPose(1),RightFront);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,10));
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0,10));

	//2
	s = new Sonar(2,m_sonarProxy->GetPose(2),RightFront);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,20));
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0,5));

	//3
	s = new Sonar(3,m_sonarProxy->GetPose(3),Front);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0.25,45));
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0.20,40));
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0.20,40));

	//4
	s = new Sonar(4,m_sonarProxy->GetPose(4),Front);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0.5,0));
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0.4,0));
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0.4,0));

	//5
	s = new Sonar(5,m_sonarProxy->GetPose(5),Front);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0.25,-45));
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0.20,-40));
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0.20,-40));

	//6
	s = new Sonar(6,m_sonarProxy->GetPose(6),LeftFront);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,-20));
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0,-5));

	//7
	s = new Sonar(7,m_sonarProxy->GetPose(7),LeftFront);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,-10));
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0,-10));

	//8
	s = new Sonar(8,m_sonarProxy->GetPose(8),Left);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,-20));
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0,-25));

	//9
	s = new Sonar(9,m_sonarProxy->GetPose(9),Back);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,-20));

	//10
	s = new Sonar(10,m_sonarProxy->GetPose(10),Back);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(Random,Vecteur(0,20));

	//11
	s = new Sonar(11,m_sonarProxy->GetPose(11),RightBack);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0,-10));

	//12
	s = new Sonar(12,m_sonarProxy->GetPose(12),LeftBack);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0,10));

	//13
	s = new Sonar(13,m_sonarProxy->GetPose(13),RightBack);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(FollowAtRight,Vecteur(0,-5));

	//14
	s = new Sonar(14,m_sonarProxy->GetPose(14),LeftBack);
	m_groups.addSonarToGroup(s);
	m_sonars.push_back(s);
	s->defineVecteurToAlgo(FollowAtLeft,Vecteur(0,5));


}

void Robot::process(){
	//Initialisation à 0
	m_newspeed = 0;
	m_newrotation = 0;




	//Choisi l'algo utilisé
	switch(m_state){
	case Random:
		this->random();
		break;
	case FollowAtLeft:
		this->followLeft();
		break;
	case FollowAtRight:
		this->followRight();
		break;
	default:
		break;
	}

	//Met à jour la vitesse du robot
	m_p2dProxy->SetSpeed(m_newspeed,m_newrotation*M_PI/180);
}

void Robot::random(){

	//Coefficient permettant d'accélérer ou de ralentir selon la proximité
	static double CoeffProximit[] = {1.5,1,0.3,0.1,0.01};

	double d;
	Vecteur vec;
	for(unsigned int i = 0;i <m_sonars.size();i++){

		 d = m_sonars.at(i)->distance();
		 vec = m_sonars.at(i)->getVecteurToAlgo(Random);

		 //Calcul la vitesse de déplacement en mulitpliant la composante magnitude par la distance du capteur normalisé et
		 	 //par un coefficient de proximité
		 m_newspeed += vec.m() * d/MAX_DISTANCE_SONAR * CoeffProximit[m_sonars.at(i)->prox()];

		 //Calcul la vitesse de rotation en mutlipliant la composante direction par la distance proportionnel à d
		 m_newrotation += vec.d() *(MAX_DISTANCE_SONAR-d)/MAX_DISTANCE_SONAR;
	}
}

void Robot::followLeft(){

	//Coefficient permettant de ralentir selon la proximité
	static double CoeffProximit[] = {0.7,0.4,0.2,0.1,0.01};

	//Regroupement des capteurs front, leftfront et leftback
	vector<Sonar*> regroupement;
	vector<Sonar*> *sonars = m_groups.getSonarGroup(LeftFront);

	for(unsigned int i = 0; i< sonars->size();i++){
		regroupement.push_back(sonars->at(i));
	}
	sonars = m_groups.getSonarGroup(LeftBack);

	for(unsigned int i = 0; i< sonars->size();i++){
		regroupement.push_back(sonars->at(i));
	}
	sonars = m_groups.getSonarGroup(Front);

	for(unsigned int i = 0; i< sonars->size();i++){
	regroupement.push_back(sonars->at(i));
	}

	//Traitement du regroupement de capteurs
	double d;
	Vecteur vec;

	for(unsigned int i = 0;i <regroupement.size();i++){

		 d = regroupement.at(i)->distance();
		 vec = regroupement.at(i)->getVecteurToAlgo(FollowAtLeft);

		 //Calcul la vitesse de déplacement en mulitpliant la composante magnitude par la distance du capteur normalisé et
		 	 //par un coefficient de proximité
		 m_newspeed += vec.m() * d/MAX_DISTANCE_SONAR * CoeffProximit[regroupement.at(i)->prox()];

		 //Calcul la vitesse de rotation en mutlipliant la composante direction par la distance proportionnel à d
		 m_newrotation += vec.d() *(MAX_DISTANCE_SONAR-d)/MAX_DISTANCE_SONAR;
	}

	//Traitement Capteur coté gauche
	sonars = m_groups.getSonarGroup(Left);
	d = sonars->at(0)->distance();
	vec = sonars->at(0)->getVecteurToAlgo(FollowAtLeft);

	 //Calcul la vitesse de déplacement en mulitpliant la composante magnitude par la distance du capteur normalisé et
	 	 //par un coefficient de proximité
	m_newspeed += vec.m() * d/MAX_DISTANCE_SONAR * CoeffProximit[sonars->at(0)->prox()];

	//Calcul la vitesse de rotation en multipliant la composante magnitude par un écart au mur
	m_newrotation += vec.d()*(DIST_TO_FOLLOW-d); //Permet de rester à une distance du mur
}

void Robot::followRight(){
	//Coefficient permettant de ralentir selon la proximité
	static double CoeffProximit[] = {0.7,0.4,0.2,0.1,0.01};

	//Regroupement des capteurs front, rightfront et rightback
	vector<Sonar*> regroupement;
	vector<Sonar*> *sonars = m_groups.getSonarGroup(RightFront);

	for(unsigned int i = 0; i< sonars->size();i++){
		regroupement.push_back(sonars->at(i));
	}
	sonars = m_groups.getSonarGroup(RightBack);

	for(unsigned int i = 0; i< sonars->size();i++){
		regroupement.push_back(sonars->at(i));
	}
	sonars = m_groups.getSonarGroup(Front);

	for(unsigned int i = 0; i< sonars->size();i++){
	regroupement.push_back(sonars->at(i));
	}

	//Traitement du regroupement de capteurs
	double d;
	Vecteur vec;

	for(unsigned int i = 0;i <regroupement.size();i++){

		 d = regroupement.at(i)->distance();
		 vec = regroupement.at(i)->getVecteurToAlgo(FollowAtRight);

		 //Calcul la vitesse de déplacement en mulitpliant la composante magnitude par la distance du capteur normalisé et
		 	 //par un coefficient de proximité
		 m_newspeed += vec.m() * d/MAX_DISTANCE_SONAR * CoeffProximit[regroupement.at(i)->prox()];

		 //Calcul la vitesse de rotation en mutlipliant la composante direction par la distance proportionnel à d
		 m_newrotation += vec.d() *(MAX_DISTANCE_SONAR-d)/MAX_DISTANCE_SONAR;
	}

	//Traitement Capteur coté droit
	sonars = m_groups.getSonarGroup(Right);
	d = sonars->at(0)->distance();
	vec = sonars->at(0)->getVecteurToAlgo(FollowAtRight);

	 //Calcul la vitesse de déplacement en mulitpliant la composante magnitude par la distance du capteur normalisé et
	 	 //par un coefficient de proximité
	m_newspeed += vec.m() * d/MAX_DISTANCE_SONAR * CoeffProximit[sonars->at(0)->prox()];

	//Calcul la vitesse de rotation en multipliant la composante magnitude par un écart au mur
	m_newrotation += vec.d()*(DIST_TO_FOLLOW-d); //Permet de rester à une distance du mur
}
